# -*- coding: utf-8 -*-
__author__ = 'xujianhua'
import sys,os
sys.path.append(os.path.dirname(sys.path[0]))
import numpy as np
import cv2
from cv2 import *
from lcopa import calculate_codev,load_uphi_data,lcopa_usb
u_phi = load_uphi_data.U_PHI
u_phi.load_uphi("./u-phi/DF2015-FA02.txt")


def parse_pic():
    cap = cv2.VideoCapture(0)
    cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,1280);
    cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,720);
    cap.set(cv2.cv.CV_CAP_PROP_FPS,10);
    cal_frame = 0
    frame_err = 0.0
    toal_angle = 0.0
    MAX_FRAME = 10
    last_ave = 0
    lcopausb = lcopa_usb.LcopaUsb()
    lcopausb.open(0x1234)
    while(True):
        # Capture frame-by-frame
        ret, frame = cap.read()
        gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)

        #kernel = np.ones((3,3),np.float32)/9
        #filter_frame = cv2.filter2D(gray,-1,kernel)
        # Display the resulting frame
        #result_arry = np
        # .ones((width,height,3),np.uint8,1)
        # result = cv2.cv.CreateMat(480,640,cv2.CV_16UC3)
        # src = cv2.cv.fromarray(hsv)
        # cv2.cv.Convert(src,result)
        #result =  cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)

        #mylist = [[result[i,j] for i in range(width)] for j in range(height)]
        #result_frame = np.array(mylist)
        #cv2.imshow('frame',result)


        kernel = np.ones((5,5),np.float32)/25
        filter_frame = cv2.medianBlur(gray,41)#cv2.filter2D(dst_img,-1,kernel)  #41
        #cv2.imshow('filter_frame',filter_frame)
        hist = np.average(filter_frame.ravel())
        _,out=cv2.threshold(filter_frame,240,255,cv2.THRESH_BINARY)
        cv2.imshow('out1',out)
        # # cv2.imshow('out1',out)
        contours, hierarchy = cv2.findContours(out,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
        #print "------------------------------------------contours-----------------------------------------------\n",(contours)
        #cv2.drawContours(frame,contours,-1,(0,255,0),-1)
        #cv2.imshow('out',frame)
        # # print "out",out.shape

        #print "out",out
        # #fs = open('data.txt', 'w')
        # #S='\n'.join(str(num)[1:-1] for num in filter_frame)
        # #fs.write(S)
        # #fs.close()
        sumx=0
        sumy=0
        area=0
        max_area = 0
        max_x = 0
        max_y = 0
        for i in range(0,len(contours)):
            area = cv2.contourArea(contours[i])
            if area >  max_area:
                center,r = cv2.minEnclosingCircle(contours[i])
                max_area = area
                frame_err += (float(center[0] - 640))*0.0000040
                #print 'center,r',center,r
                if(cal_frame == MAX_FRAME):
                    cal_frame = 0
                    ave = frame_err/MAX_FRAME
                    toal_angle += ave
		    print '(last_ave - ave):',(last_ave - ave)
		    toal_angle += (last_ave - ave)*0.075
                    last_ave = ave
                    print 'center,r',center,r
                    print 'ave:',ave

                    print 'toal_angle:',toal_angle
                    frame_err = 0
                    btyes=calculate_codev.calculate_voltage((toal_angle))
		    lcopausb.send_byte(btyes)
                else:
                    cal_frame += 1
        if max_area != 0:
            cv2.drawContours(frame,contours,-1,255)
                #cv2.circle(frame,(int(center[0]),int(center[1])),int(r),255,-1)
                #print "x-y",(sumy/area,sumx/area)
        cv2.imshow('image',frame)
        #cv2.imshow('filter_frame',filter_frame)
        #cv2.imshow('result frame',result)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    # When everything done, release the capture
    cap.release()
    cv2.destroyAllWindows()
